#include "prospeckz_radio.h"
#include "StopAndWait.h"
#include "MAC.h"
#include "prospeckz.h"

#include "defines.h"

#include "hashmap.h"
#include "dynmem.h"


#define MAX_RX_SIZE 79

#define TRANSMIT_INVALID 0
#define TRANSMIT_IDLE 1
#define TRANSMIT_WAITING 2
#define TRANSMIT_TIMEOUT 3
#define TRANSMIT_ACKD 4

size_t hash_addr64(void *key)
{
	struct xlowpan_addr64 *addr = (struct xlowpan_addr64*)key;
	size_t hash = 0;

	if(!key)
		return 0;

	/* TODO: make better hashing function -> use Fletcher-16 ? */

	/* lame */
	{
		size_t i = XLOWPAN_ADDR_LEN;
		while(i--) {
			hash += addr->addr[i] * i;
		}
	}

	return hash;
}

void outputAddr(struct xlowpan_addr64* addr)
{
/*	char buffer[4];
	int i;
	UART_CPutString("Address: ");
	
	for(i=0;i<8;i++)
	{
		utoa(buffer, addr->addr[i],10);
		UART_PutString(buffer);
		if(i < 7)UART_CPutString(".");
	}
	
	UART_CPutString("\r\n");
	*/
}

struct reliable_status
{
	unsigned char TransmitStatus;
	char NextSendSeq;
	char NextRecvSeq;
} ;

struct hash_map* reliable_status_map;

char SendTimeout = 0;

unsigned int (*Reliable_SendTarget)(struct xlowpan_addr64* from, void* data, unsigned int length);
void (*Reliable_ReceiveTarget)(struct xlowpan_addr64* from, unsigned char* data, unsigned int length);

void Reliable_SetReceiveHandler(void(*X)(struct xlowpan_addr64* from, unsigned char* data, unsigned int length))
{
	Reliable_ReceiveTarget = X;
}

void Reliable_SetSendHandler(unsigned int (*X)(struct xlowpan_addr64* to, void* data, unsigned int length))
{
	Reliable_SendTarget = X;
}

struct xlowpan_addr64* Addr;
void Reliable_GetAddr(struct xlowpan_addr64* target)
{
	target->addr[0] = Addr->addr[0];
	target->addr[1] = Addr->addr[1];
	target->addr[2] = Addr->addr[2];
	target->addr[3] = Addr->addr[3];
	target->addr[4] = Addr->addr[4];
	target->addr[5] = Addr->addr[5];
	target->addr[6] = Addr->addr[6];
	target->addr[7] = Addr->addr[7];
}

unsigned char Reliable_MakeSession(void)
{
	static unsigned char sess = 0;
	
	return sess++;
}

struct mac_driver layer;

void Reliable_Init(struct xlowpan_addr64* addr)
{
	
	reliable_status_map = hmap_create(10, hash_addr64);
	
	Addr = addr;
	
	layer.send_pkt = &MAC_Transmit;
	layer.set_receive_pkt = &MAC_SetHandler;
	layer.get_addr64 = &Reliable_GetAddr;
	layer.make_session = &Reliable_MakeSession;

	/*
	SAWTimer_EnableInt();
	SAWTimer_Stop();
	*/
	
	Radio_init(254);
	Radio_setTxStrength(5);
	Radio_receiverOn;	
	
	xlowpan_init(&layer);
}

unsigned char fullPacket[79];
int Reliable_TransmitPacket(struct xlowpan_addr64* to, unsigned char* data, unsigned int length, unsigned int timeout, unsigned int retries)
{
	int i = 0;
	int check_fail = 0;

	struct reliable_status* status;
	
	if(length > 77)
	{
		return 2;
	}

	status = hmap_get(reliable_status_map, to);

	if(status == 0)
	{
		outputAddr(to);
		status = (struct reliable_status*)malloc(sizeof(struct reliable_status));
		if(status == 0)
		{
			return 3;
		}
		status->TransmitStatus = TRANSMIT_IDLE;
		status->NextSendSeq = 0;
		status->NextRecvSeq = 0;
		hmap_set(reliable_status_map, to, status);
	}

	fullPacket[0] = 0;
	fullPacket[1] = status->NextSendSeq;

	memcpy(fullPacket+2, data, length);
	
	status->TransmitStatus = TRANSMIT_WAITING;
		
	Reliable_SendTarget(to, fullPacket,length+2);

	/*
	SAWTimer_WritePeriod(timeout);
	SAWTimer_Start();
	*/
	
	retries--;
	SendTimeout = 0;
	
//	DEBUGLOGC("sending packet\r\n");
	
	while(status->TransmitStatus != TRANSMIT_ACKD)
	{
		while(MAC_CheckRadio());
		
		if(SendTimeout)
		{
	//		DEBUGLOGC("Timeout\r\n");
			if(check_fail)
			{
				if(retries > 0)
				{
			//		DEBUGLOGC("Retrying\r\n");
					status->TransmitStatus = TRANSMIT_WAITING;
					Reliable_SendTarget(to, fullPacket,length+2);
					
					/* SAWTimer_Start(); */
					retries--;
				}
				else
				{
			//		DEBUGLOGC("Failed\r\n");
					status->TransmitStatus = TRANSMIT_IDLE;
					/* SAWTimer_Stop(); */
					return 1;
				}
			}
			
			check_fail = !check_fail;
			SendTimeout = 0;
		}

	}
	//DEBUGLOGC("Success\r\n");
	status->TransmitStatus = TRANSMIT_IDLE;
	return 0;
}

void Reliable_ReceivePacket(struct xlowpan_addr64* from, unsigned char* data, unsigned int length)
{
	
	int i = 0;
	unsigned char ack[2];
	struct reliable_status *status;

	if(length < 2) return; //no data

	status = hmap_get(reliable_status_map, from);
	
	if(status == 0)
	{
		
		status = (struct reliable_status*) malloc(sizeof(struct reliable_status));
		if(status == 0)
		{
			return;
		}
		
		status->TransmitStatus = TRANSMIT_IDLE;
		status->NextSendSeq = 0;
		status->NextRecvSeq = 0;
		hmap_set(reliable_status_map, from, status);
	}
	else
	{
		outputAddr(from);
	}

	if(data[0] == 1)
	{
		if((status->TransmitStatus == TRANSMIT_WAITING || status->TransmitStatus == TRANSMIT_TIMEOUT) && data[1] == status->NextSendSeq)
		{
			status->NextSendSeq = ! status->NextSendSeq;
			status->TransmitStatus = TRANSMIT_ACKD;
		/*	SAWTimer_Stop(); */
		}
	}
	
	else if(data[0] == 0)
	{
	
	
		if(data[1] == status->NextRecvSeq)
		{
			ack[0] = 1;
			ack[1] = status->NextRecvSeq;
			status->NextRecvSeq = !status->NextRecvSeq;
			
			
			Reliable_SendTarget(from, ack,2);
			
			Reliable_ReceiveTarget(from, data+2,length-2);
			
		}
		else
		{
			ack[0] = 1;
			ack[1] = ! status->NextRecvSeq;
			
			Reliable_SendTarget(from, ack,2);
		}
	}
}
/*
#pragma interrupt_handler SAWTimer_ISR
void SAWTimer_ISR(void)
{
	SendTimeout = 1;
}
*/